/*
 * Copyright (C) 2006-2012 TongYan Corporation
 * All rights reserved.
 *
 * @brief info:
 *
 * @author: 王哲成 - wangzhecheng@yeah.net
 * @date: 2021.05.25
 * @last modified: 2021-05-25 10:32
 *
 */

#include "../include/ISGObjectFunction.hpp"

// 计算雅可比向量Jx（梯度向量）
Vector ISGObjectFunction::gradientVector(const Vector &x,
                                         const Vector &dx) const {
  size_t n = x.size();
  Vector Jx = Vector::Zero(n);

  Vector delta;
  if (dx.size() == 0)
    delta = x.cwiseAbs() * 1e-2;
  else
    delta = dx;
  assert(x.size() == delta.size());

  for (size_t i = 0; i != n; i++) {
    Jx[i] = firstDeriv(x, i, delta[i]);
  }
  return Jx;
}

Matrix ISGObjectFunction::HessianMatrix(const Vector &x,
                                        const Vector &dx) const {
  size_t n = x.size();
  Matrix Hx = Matrix::Zero(n, n);

  Vector delta;
  if (dx.size() == 0)
    delta = x.cwiseAbs() * 1e-2;
  else
    delta = dx;
  assert(x.size() == delta.size());

  for (size_t i = 0; i != n; i++)
    for (size_t j = 0; j != n; j++)
      Hx(i, j) = secondDeriv(x, i, delta[i], j, delta[j]);

  return Hx;
}
// 中心差分法得一阶偏微分
double ISGObjectFunction::firstDeriv(const Vector &x, const size_t &loc,
                                     const double &delta) const {
  Vector dx = Eigen::VectorXd::Zero(x.size());
  dx[loc] += std::max(delta, 1e-2);

  double f_xplus = functionValue(x + dx);
  double f_xminus = functionValue(x - dx);

  return (f_xplus - f_xminus) / (2. * delta);
};
// 中心差分法得二阶偏微分(对同一变量两次求偏导)
double ISGObjectFunction::secondDeriv(const Vector &x, const size_t &loc,
                                      const double &delta) const {
  Vector dx = Eigen::VectorXd::Zero(x.size());
  dx[loc] += std::max(delta, 1e-2);

  double f_xplus = functionValue(x + dx);
  double f_xminus = functionValue(x - dx);
  double f_x = functionValue(x);

  return (f_xplus + f_xminus - 2 * f_x) / (delta * delta);
}
// 中心差分法得二阶偏微分(对不同变量两次求偏导)
double ISGObjectFunction::secondDeriv(const Vector &x, const size_t &loc_x,
                                      const double &delta_x,
                                      const size_t &loc_y,
                                      const double &delta_y) const {
  if (loc_x == loc_y)
    return secondDeriv(x, loc_x, delta_x);

  Vector dy = Eigen::VectorXd::Zero(x.size());
  dy[loc_y] += std::max(delta_y, 1e-2);

  double fx_yplus = firstDeriv(x + dy, loc_x, delta_x);
  double fx_yminus = firstDeriv(x - dy, loc_x, delta_x);

  return (fx_yplus - fx_yminus) / (2. * delta_y);
};

Vector ISGLeastSquareProblem::gradientVector(const Vector &x,
                                             const Vector &dx) const {
  Vector x0, x1;
  x0.resizeLike(x);
  x1.resizeLike(x);

  for(auto pt:m)
}

Matrix ISGLeastSquareProblem::HessianMatrix(const Vector &x,
                                            const Vector &dx) const {}
